#ifndef __FILT_HPP__
#define __FILT_HPP__

#include <string> // std::stod
#include <vector>
#include <list>
#include <array>
#include <iostream>
#include <fstream>
#include <initializer_list>
#include <type_traits>

namespace kb
{
namespace math
{

template <typename T>
/**
 * @struct FilterCoefficients
 * @author kbaur
 * @date 2022
 */
struct FilterCoefficients {
  static_assert(std::is_floating_point<T>::value, "Template must be a floating point type!");
  std::vector<T> m_CoefficientsA;
  std::vector<T> m_CoefficientsB;
  /**
   * @brief
   *
   * @param p_ostream
   * @param p_filterCoefficients
   * @return std::ostream&
   */
  friend std::ostream& operator<<(std::ostream& p_ostream, FilterCoefficients& p_filterCoefficients)
  {
    std::vector<std::vector<T>> vec{p_filterCoefficients.m_CoefficientsA, p_filterCoefficients.m_CoefficientsB};

    for (std::vector<T>& a : vec) {
      for (int i = 0; i < a.size() - 1; ++i) {
        p_ostream << a.at(i) << ",";
      }

      p_ostream << *(a.end() - 1);
      p_ostream << std::endl;
    }

    return p_ostream;
  }
  /**
   * @brief
   *
   * @param p_istream
   * @param p_filterCoefficients
   * @return std::istream&
   */
  friend std::istream& operator>>(std::istream& p_istream, FilterCoefficients& p_filterCoefficients)
  {
    char input;
    bool end{false};
    std::string buff;
    std::vector<double> currentVector;
    double val;
    int counter = 0;

    while (p_istream.get(input) && counter < 248) {
      if (input == '\n' || input == ',' || input == ';') {
        val = std::stod(buff);
        buff.clear();
        currentVector.push_back(val);

        if (input == '\n') {
          p_filterCoefficients.m_CoefficientsB = currentVector;
          end = true;
        } else if (input == ';') {
          p_filterCoefficients.m_CoefficientsA = currentVector;
          currentVector.clear();
        }
      } else {
        buff.push_back(input);
      }

      ++counter;
    }

    return p_istream;
  }
};

template <typename T>
/**
 * @class Filt
 * @details Digital filter class
 * @author kbaur
 * @date 2022
 */
class Filt
{
public:
  static_assert(std::is_floating_point<T>::value, "Template parameter must be a floating point type!");
  /**
   * @brief Construct a new Filt object
   *
   * @param p_coefficients
   */
  Filt(const FilterCoefficients<T>& p_coefficients);
  /**
   * @brief Construct a new Filt object
   *
   * @param p_coefficients
   */
  Filt(const Filt& p_coefficients);
  /**
   * @brief copy
   *
   * @param p_filt
   * @return Filt<T>&
   */
  Filt<T>& operator=(const Filt& p_filt);
  /**
   * @brief Filter
   *
   * @param p_inputVector
   * @return std::vector<T>&&
   */
  std::vector<T> &&Filter(const std::vector<T>& p_inputVector);
  /**
   * @brief Filter
   *
   * @param p_inputVector
   * @param p_initialState
   * @return std::vector<T>&&
   */
  std::vector<T> &&Filter(const std::vector<T>& p_inputVector, const std::vector<T>& p_initialState);
  /**
   * @brief operator()
   *
   * @param p_inputVector
   * @return std::vector<T>&&
   */
  std::vector<T> &&operator()(const std::vector<T>& p_inputVector);
  /**
   * @brief Get the Coefficients object
   *
   * @return FilterCoefficients<T>
   */
  FilterCoefficients<T> GetCoefficients() const;
  /**
   * @brief Get the Output object
   *
   * @return std::vector<T>
   */
  std::vector<T> GetOutput() const;
  template <typename U>
  /**
   * @brief Set the Coefficients object
   *
   * @param p_coefficients
   */
  void SetCoefficients(U &&p_coefficients);

protected:
  T CalculateOneStep(T input);
  FilterCoefficients<T> m_FilterCoefficients;
  std::vector<T> m_Output;
  std::vector<T> m_Delay;
};
}
}

namespace kb
{
namespace math
{

template <typename T>
Filt<T>& Filt<T>::operator=(const Filt& p_filt)
{
  m_FilterCoefficients = p_filt.GetCoefficients();
  m_Output = p_filt.GetOutput();
  return *this;
}

template <typename T>
Filt<T>::Filt(const Filt& p_filt)
{
  m_FilterCoefficients = p_filt.GetCoefficients();
  m_Output = p_filt.GetOutput();
}

template <typename T>
Filt<T>::Filt(const FilterCoefficients<T>& p_coefficients)
{
  SetCoefficients(p_coefficients);
}

template <typename T>
T Filt<T>::CalculateOneStep(T p_input)
{
  // TODO check for overflow
  T output = m_FilterCoefficients.m_CoefficientsB.at(0) * p_input + m_Delay.at(0);

  for (size_t j = 1; j < m_FilterCoefficients.m_CoefficientsA.size(); ++j) {
    m_Delay.at(j - 1) = m_FilterCoefficients.m_CoefficientsB.at(j) * p_input + m_Delay.at(j) - m_FilterCoefficients.m_CoefficientsA.at(j) * output;
  }

  return output;
}

template <typename T>
std::vector<T> &&Filt<T>::Filter(const std::vector<T>& p_inputVector)
{
  return Filter(p_inputVector, std::vector<T>(m_FilterCoefficients.m_CoefficientsA.size() - 1, 0.0));
}

template <typename T>
std::vector<T> &&Filt<T>::Filter(const std::vector<T>& p_inputVector, const std::vector<T>& p_initialState)
{
  if (p_inputVector.size() < 1) {
    throw std::invalid_argument("Signal must be larger than 0");
  }

  if (p_initialState.size() < m_FilterCoefficients.m_CoefficientsA.size() - 1) {
    throw std::invalid_argument("Initial states must have coefficient size-1");
  }

  const size_t order = m_FilterCoefficients.m_CoefficientsA.size();
  m_Output = std::vector<T>(p_inputVector.size(), 0);
  m_Delay = std::move(p_initialState);
  m_Delay.push_back(0);

  // TODO Parallelize if possible
  for (size_t i = 0; i < p_inputVector.size(); ++i) {
    m_Output.at(i) = CalculateOneStep(p_inputVector.at(i));
  }

  return std::move(m_Output);
}

template <typename T>
std::vector<T> &&Filt<T>::operator()(const std::vector<T>& p_inputVector)
{
  return Filter(p_inputVector);
}

template <typename T>
FilterCoefficients<T> Filt<T>::GetCoefficients() const
{
  return m_FilterCoefficients;
}

template <typename T>
template <typename U>
void Filt<T>::SetCoefficients(U &&p_coefficients)
{
  m_FilterCoefficients = std::forward<U>(p_coefficients);

  if (m_FilterCoefficients.m_CoefficientsA.size() != m_FilterCoefficients.m_CoefficientsB.size()) {
    throw std::invalid_argument("Coefficients of A and B must have the same size!");
  }
}

template <typename T>
std::vector<T> Filt<T>::GetOutput() const
{
  return m_Output;
}

}
}

#endif // __FILT_HPP__